#include "controller.h"
#include "data_simulator.h"
#include <iostream>
#include <chrono>
#include "math_.h"
#include "track_3d.h"

Controller::Controller()
{

    // 参数初始化
    m_params.carrier_frequency = 10e9f;  // 10 GHz
    m_params.pulse_width = 1e-6f;        // 1 μs
    m_params.pulse_rep_interval = 1e-3f; // 1 ms
    m_params.num_pulses = 16;            // 64;
    m_params.num_samples = 1024;
    m_params.sampling_rate = 10e6f; // 10 MHz
    m_params.bandwidth = 5e6f;      // 5 MHz
    m_params.num_antennas = 8;
    m_params.antenna_spacing = 0.015f; // 15 mm
    m_params.light_speed = 3e8f;       // 光速

    // 处理器初始化
    m_radarProcessor = std::make_unique<RadarProcessor>(m_params);
    m_dataSimulator = std::make_unique<DataSimulator>(m_params);
    m_track3d = std::make_unique<Track3D>();
}

#if 0
 // 目标信息结构
struct TargetInfo
{
    float range;        // 距离 (m)
    float velocity;     // 速度 (m/s)
    float angle;        // 角度 (度)
    float power;        // 功率 (dB)
    float snr;          // 信噪比 (dB)
    int num_detections; // 凝聚的点迹数量

    TargetInfo(float r, float v, float a, float p, float s, int n)
        : range(r), velocity(v), angle(a), power(p), snr(s), num_detections(n) {}
};
#endif

#if 0
// 目标航迹信息
typedef struct
{
    uint32_t id;         // 航迹ID
    TargetState3D state; // 当前状态
    KalmanFilter3D kf;   // 卡尔曼滤波器
    uint8_t life;        // 航迹寿命
    uint8_t max_life;    // 最大寿命（超过则删除）
    bool is_valid;       // 航迹是否有效
} Tracks3D;

#endif

void Controller::init()
{

    //------------------------------接入数据部分---------------------------------------------------------

    //==================================检测==================================================================++++++
    // 定义模拟目标
    std::vector<TargetInfo> simulated_targets = {
        {5000.0f, 100.0f, 30.0f, 60.0f, 0.0f, 0}, // 5km, 100m/s, 30°, 60dB
                                                  // {8000.0f, -50.0f, -10.0f, 55.0f, 0.0f, 0}, // 8km, -50m/s, -10°, 55dB
        //{3000.0f, 0.0f, 5.0f, 50.0f, 0.0f, 0}      // 3km, 0m/s, 5°, 50dB
    };

    // 生成模拟数据
    auto simulated_data = m_dataSimulator->generateData(simulated_targets);

    // 处理数据
    auto detected_targets = m_radarProcessor->process(simulated_data);
    //[[],[],[]]

    // 初始化轨迹 8条轨迹
    Tracks3D tracks[MAX_TRACKS_3D];

    m_track3d->track3d_init(tracks);

    //******************************************检测和追踪目标转化******************************************************* */

#if 0
    double r;           // 径向距离
    double theta;       // 方位角（xy平面，从x轴逆时针为正）
    double phi;         // 俯仰角（xz平面，从x轴向上为正）
    uint64_t timestamp; // 时间戳(ms)
#endif

    // 检测到一帧数据
    //  输出结果
    // std::vector<RadarPoint3D> one_frame;

    RadarPoint3D frame[2];
    std::vector<RadarPoint3D>one_frame;

    std::cout << "检测到的目标:" << std::endl;
    int i = 0;
    for (const auto &target : detected_targets)
    {
        std::cout << "距离: " << target.range << " m, "
                  << "速度: " << target.velocity << " m/s, "
                  << "角度: " << target.angle << " deg, "
                  << "功率: " << target.power << " dB, "
                  << "SNR: " << target.snr << " dB, "
                  << "点迹数: " << target.num_detections << std::endl;

        if(i==2){
            break;
        }


        RadarPoint3D rp3;
        rp3.r = target.range;
        rp3.theta = target.angle;
        rp3.phi = 30; // 这个地方需要重新实现俯仰角算法
        // 时间辍有问题，应该是在检测处添加时间辍
        auto now = std::chrono::system_clock::now();
        rp3.timestamp = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();
        frame[i]=rp3;

        i++;
    }
  
    std::cout<<"===================================start track=========================================================="<<std::endl;
   
    m_track3d->radar3d_track_process(frame,2,tracks);
    std::cout << "total point:" << i << std::endl;

//===================================追踪==========================================================



}